import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
import tf_transformations
from turtlesim.msg import Pose


class FramePublisher(Node):
    def __init__(self):
        super().__init__("turtle_tf2_frame_publisher")
        self.declare_parameter("turtlename", "turtle1")
        self.turtlename = self.get_parameter("turtlename").get_parameter_value().string_value
        self.br = TransformBroadcaster(self)
        self.subscription = self.create_subscription(Pose, f"/{self.turtlename}/pose", self.handle_turtle_pose, 1)
        self.subscription

    def handle_turtle_pose(self, msg):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = "world"
        t.child_frame_id = self.turtlename

        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        q = tf_transformations.quaternion_from_euler(0, 0, msg.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        self.br.sendTransform(t)


def main():
    rclpy.init()
    node = FramePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()


if __name__ == "__main__":
    main()
